#!/usr/bin/env python3

import rospy
from DrEmpower_can.msg import motion_follow_aid
# import DrEmpower as dr

def callback(data):
    import DrEmpower as dr
    dr.motion_aid(id_num=data.id_num, angle=data.angle, speed=data.speed, angle_err=data.angle_err, 
                  speed_err=data.speed_err, torque=data.torque)
    rospy.loginfo("motion_follow_aid_node")


def listener():
    rospy.init_node("motion_follow_aid", anonymous=True)
    rospy.Subscriber("motion_follow_aid", motion_follow_aid, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

